/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "cartographer/mapping/2d/probability_grid.h"

#include <limits>

#include "absl/memory/memory.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/mapping/submaps.h"

namespace cartographer
{
	namespace mapping
	{
		ProbabilityGrid::ProbabilityGrid(const MapLimits& limits, ValueConversionTables* conversion_tables)
			: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
				conversion_tables),
			conversion_tables_(conversion_tables) {}

		ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ValueConversionTables* conversion_tables)
			: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
			CHECK(proto.has_probability_grid_2d());
		}

		// Sets the probability of the cell at 'cell_index' to the given
		// 'probability'. Only allowed if the cell was unknown before.
		void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, const float probability) {
			//在调用该函数之前需要保证对应的栅格单元的概率是未知的
			uint16& cell = (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
			CHECK_EQ(cell, kUnknownProbabilityValue);
			//函数ProbabilityToCorrespondenceCost实际上是对输入概率值取反，这意味着如果我们的输入描述
			//的是栅格单元的占用概率，那么实际存储的则是栅格单元空闲的概率。
			//函数CorrespondenceCostToValue是将float类型的数据转换为uint16类型，并将输入从区间
			//[kMinCorrespondenceCost,kMaxCorrespondenceCost]映射到[1,32767]。
			cell = CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
			mutable_known_cells_box()->extend(cell_index.matrix());
		}

		// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
		// to the probability of the cell at 'cell_index' if the cell has not already
		// been updated. Multiple updates of the same cell will be ignored until
		// FinishUpdate() is called. Returns true if the cell was updated.
		//
		// If this is the first call to ApplyOdds() for the specified cell, its value
		// will be set to probability corresponding to 'odds'.
		/*
		* 通过查表来更新栅格单元的占用概率。这也就是为什么会费那么多精力把浮点数转换为uint16。
		* cell_index是将要更新的栅格单元索引，table是更新过程中将要查的表。
		*/
		bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector<uint16>& table) {
			// 先检查查找表的大小
			DCHECK_EQ(table.size(), kUpdateMarker);
			// 通过cell_index计算栅格单元的存储索引，获取对应的存储值，并确保该值不会超出查找表的数组边界
			const int flat_index = ToFlatIndex(cell_index);
			uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
			if (*cell >= kUpdateMarker) {
				return false;
			}
			mutable_update_indices()->push_back(flat_index);
			*cell = table[*cell];
			DCHECK_GE(*cell, kUpdateMarker);
			mutable_known_cells_box()->extend(cell_index.matrix());
			return true;
		}

		GridType ProbabilityGrid::GetGridType() const {
			return GridType::PROBABILITY_GRID;
		}

		/*
		* Probability是指栅格被占用的概率，而CorrespondenceCost则是栅格空闲的概率。
		*/
		// Returns the probability of the cell with 'cell_index'.
		float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const
		{
			//判定输入的单元索引是否在子图的范围内，如不在就直接返回一个最小的概率。 
			if (!limits().Contains(cell_index)) return kMinProbability;
			return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
				correspondence_cost_cells()[ToFlatIndex(cell_index)]));
		}

		proto::Grid2D ProbabilityGrid::ToProto() const {
			proto::Grid2D result;
			result = Grid2D::ToProto();
			result.mutable_probability_grid_2d();
			return result;
		}
		/*
		* 用于裁剪子图,在更新子图的过程中，并不能保证更新的数据能够完整覆盖整个子图的所有栅格。
		*/
		std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
			Eigen::Array2i offset;
			CellLimits cell_limits;
			ComputeCroppedLimits(&offset, &cell_limits);
			const double resolution = limits().resolution();
			const Eigen::Vector2d max =
				limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
			std::unique_ptr<ProbabilityGrid> cropped_grid =
				absl::make_unique<ProbabilityGrid>(
					MapLimits(resolution, max, cell_limits), conversion_tables_);
			for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
				if (!IsKnown(xy_index + offset)) continue;
				cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
			}

			return std::unique_ptr<Grid2D>(cropped_grid.release());
		}

		bool ProbabilityGrid::DrawToSubmapTexture(
			proto::SubmapQuery::Response::SubmapTexture* const texture,
			transform::Rigid3d local_pose) const {
			Eigen::Array2i offset;
			CellLimits cell_limits;
			ComputeCroppedLimits(&offset, &cell_limits);

			std::string cells;
			for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
				if (!IsKnown(xy_index + offset)) {
					cells.push_back(0 /* unknown log odds value */);
					cells.push_back(0 /* alpha */);
					continue;
				}
				// We would like to add 'delta' but this is not possible using a value and
				// alpha. We use premultiplied alpha, so when 'delta' is positive we can
				// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
				// zero, and use 'alpha' to subtract. This is only correct when the pixel
				// is currently white, so walls will look too gray. This should be hard to
				// detect visually for the user, though.
				const int delta =
					128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
				const uint8 alpha = delta > 0 ? 0 : -delta;
				const uint8 value = delta > 0 ? delta : 0;
				cells.push_back(value);
				cells.push_back((value || alpha) ? alpha : 1);
			}

			common::FastGzipString(cells, texture->mutable_cells());
			texture->set_width(cell_limits.num_x_cells);
			texture->set_height(cell_limits.num_y_cells);
			const double resolution = limits().resolution();
			texture->set_resolution(resolution);
			const double max_x = limits().max().x() - resolution * offset.y();
			const double max_y = limits().max().y() - resolution * offset.x();
			*texture->mutable_slice_pose() = transform::ToProto(
				local_pose.inverse() *
				transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

			return true;
		}

	}  // namespace mapping
}  // namespace cartographer
